/*
 * Copyright (c) 2013-2022 ARM Limited. All rights reserved.
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the License); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * ----------------------------------------------------------------------
 *
 * $Date:        26. April 2022
 * $Revision:    V2.1.1
 *
 * Project:      CMSIS-DAP Source
 * Title:        DAP.c CMSIS-DAP Commands
 *
 *---------------------------------------------------------------------------*/

#include <string.h>
#include "DAP_config.h"
#include "DAP.h"
#include "esp_log.h"
#include "dap_setting.h"
#include "spi_switch.h"

#if (DAP_PACKET_SIZE < 64U)
#error "Minimum Packet Size is 64!"
#endif
#if (DAP_PACKET_SIZE > 32768U)
#error "Maximum Packet Size is 32768!"
#endif
#if (DAP_PACKET_COUNT < 1U)
#error "Minimum Packet Count is 1!"
#endif
#if (DAP_PACKET_COUNT > 255U)
#error "Maximum Packet Count is 255!"
#endif

// Clock Macros
#define MAX_SWJ_CLOCK(delay_cycles) \
    ((CPU_CLOCK / 2U) / (IO_PORT_WRITE_CYCLES + delay_cycles))

DAP_Data_t DAP_Data;                // DAP Data
volatile uint8_t DAP_TransferAbort; // Transfer Abort Flag

static const char DAP_FW_Ver[] = DAP_FW_VER;

/**
 * @brief 设置SWJ时钟延迟的计算函数
 * @param clock 请求的SWJ时钟频率（单位：Hz）
 * 
 * 该函数根据目标时钟频率计算所需的延迟周期数，用于调整SWD/JTAG通信时序。
 * 包含快速时钟模式和普通时钟模式两种处理逻辑。
 */
void Set_Clock_Delay(uint32_t clock)
{
    uint32_t delay; // 延迟周期计数器

    // 判断是否进入快速时钟模式（高于阈值频率）
    if (clock >= MAX_SWJ_CLOCK(DELAY_FAST_CYCLES)) {
        DAP_Data.fast_clock = 1U;  // 启用快速时钟模式标志
        DAP_Data.clock_delay = 1U; // 快速模式下固定使用最小延迟
    }
    // 普通时钟模式处理
    else {
        DAP_Data.fast_clock = 0U; // 禁用快速时钟模式

        /* 计算基础延迟周期数：
         * 公式原理：CPU时钟周期数 / 目标时钟周期数
         * 加上(clock-1)是为了实现四舍五入效果
         */
        /*
            CPU_CLOCK / 2	将CPU主时钟折半（因为SWD协议通常在时钟边沿采样，每个周期需要两个边沿）
            clock - 1		实现整数除法的四舍五入
            clock		    计算目标频率对应的分频系数
        */
        delay = ((CPU_CLOCK / 2U) + (clock - 1U)) / clock;

        /* 延迟周期补偿：
         * 如果计算出的延迟大于IO端口写入周期，
         * 则需要减去基础操作耗时并进行周期对齐
         */
        if (delay > IO_PORT_WRITE_CYCLES) {
            delay -= IO_PORT_WRITE_CYCLES;                                  // 补偿基础操作耗时
            delay = (delay + (DELAY_SLOW_CYCLES - 1U)) / DELAY_SLOW_CYCLES; // 按慢速周期对齐
        }
        // 确保最小延迟为1个周期
        else {
            delay = 1U;
        }

        DAP_Data.clock_delay = delay; // 存储最终计算的延迟值
    }
}

// Get DAP Information
//   id:      info identifier
//   info:    pointer to info data
//   return:  number of bytes in info data
static uint8_t DAP_Info(uint8_t id, uint8_t *info)
{
    uint8_t length = 0U;
    ESP_LOGI("Md-Link", "DAP_Info: id=%d", id);
    switch (id) {
        case DAP_ID_VENDOR:
            length = DAP_GetVendorString((char *)info);
            break;
        case DAP_ID_PRODUCT:
            length = DAP_GetProductString((char *)info);
            break;
        case DAP_ID_SER_NUM:
            length = DAP_GetSerNumString((char *)info);
            break;
        case DAP_ID_DAP_FW_VER:
            length = (uint8_t)sizeof(DAP_FW_Ver);
            memcpy(info, DAP_FW_Ver, length);
            break;
        case DAP_ID_DEVICE_VENDOR:
            length = DAP_GetTargetDeviceVendorString((char *)info);
            break;
        case DAP_ID_DEVICE_NAME:
            length = DAP_GetTargetDeviceNameString((char *)info);
            break;
        case DAP_ID_BOARD_VENDOR:
            length = DAP_GetTargetBoardVendorString((char *)info);
            break;
        case DAP_ID_BOARD_NAME:
            length = DAP_GetTargetBoardNameString((char *)info);
            break;
        case DAP_ID_PRODUCT_FW_VER:
            length = DAP_GetProductFirmwareVersionString((char *)info);
            break;
        case DAP_ID_CAPABILITIES:
            info[0] = ((DAP_SWD != 0) ? (1U << 0) : 0U) |
                      ((DAP_JTAG != 0) ? (1U << 1) : 0U) |
                      ((SWO_UART != 0) ? (1U << 2) : 0U) |
                      ((SWO_MANCHESTER != 0) ? (1U << 3) : 0U) |
                      /* Atomic Commands  */ (1U << 4) |
                      ((TIMESTAMP_CLOCK != 0U) ? (1U << 5) : 0U) |
                      ((SWO_STREAM != 0U) ? (1U << 6) : 0U) |
                      ((DAP_UART != 0U) ? (1U << 7) : 0U);

            info[1] = ((DAP_UART_USB_COM_PORT != 0) ? (1U << 0) : 0U);
            length = 2U;
            break;
        case DAP_ID_TIMESTAMP_CLOCK:
#if (TIMESTAMP_CLOCK != 0U)
            info[0] = (uint8_t)(TIMESTAMP_CLOCK >> 0);
            info[1] = (uint8_t)(TIMESTAMP_CLOCK >> 8);
            info[2] = (uint8_t)(TIMESTAMP_CLOCK >> 16);
            info[3] = (uint8_t)(TIMESTAMP_CLOCK >> 24);
            length = 4U;
#endif
            break;
        case DAP_ID_UART_RX_BUFFER_SIZE:
#if (DAP_UART != 0)
            info[0] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 0);
            info[1] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 8);
            info[2] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 16);
            info[3] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 24);
            length = 4U;
#endif
            break;
        case DAP_ID_UART_TX_BUFFER_SIZE:
#if (DAP_UART != 0)
            info[0] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 0);
            info[1] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 8);
            info[2] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 16);
            info[3] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 24);
            length = 4U;
#endif
            break;
        case DAP_ID_SWO_BUFFER_SIZE:
#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
            info[0] = (uint8_t)(SWO_BUFFER_SIZE >> 0);
            info[1] = (uint8_t)(SWO_BUFFER_SIZE >> 8);
            info[2] = (uint8_t)(SWO_BUFFER_SIZE >> 16);
            info[3] = (uint8_t)(SWO_BUFFER_SIZE >> 24);
            length = 4U;
#endif
            break;
        case DAP_ID_PACKET_SIZE:
            info[0] = (uint8_t)(DAP_PACKET_SIZE >> 0);
            info[1] = (uint8_t)(DAP_PACKET_SIZE >> 8);
            length = 2U;
            break;
        case DAP_ID_PACKET_COUNT:
            info[0] = DAP_PACKET_COUNT;
            length = 1U;
            break;
        default:
            break;
    }

    return (length);
}

// Delay for specified time
//    delay:  delay time in ms
void Delayms(uint32_t delay)
{
    delay *= ((CPU_CLOCK / 1000U) + (DELAY_SLOW_CYCLES - 1U)) / DELAY_SLOW_CYCLES;
    PIN_DELAY_SLOW(delay);
}

// Process Delay command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_Delay(const uint8_t *request, uint8_t *response)
{
    uint32_t delay;

    delay = (uint32_t)(*(request + 0)) |
            (uint32_t)(*(request + 1) << 8);
    delay *= ((CPU_CLOCK / 1000000U) + (DELAY_SLOW_CYCLES - 1U)) / DELAY_SLOW_CYCLES;

    PIN_DELAY_SLOW(delay);

    *response = DAP_OK;
    return ((2U << 16) | 1U);
}

// Process Host Status command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_HostStatus(const uint8_t *request, uint8_t *response)
{
    switch (*request) {
        case DAP_DEBUGGER_CONNECTED:
            LED_CONNECTED_OUT((*(request + 1) & 1U));
            break;
        case DAP_TARGET_RUNNING:
            LED_RUNNING_OUT((*(request + 1) & 1U));
            break;
        default:
            *response = DAP_ERROR;
            return ((2U << 16) | 1U);
    }

    *response = DAP_OK;
    return ((2U << 16) | 1U);
}
// Process Connect command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_Connect(const uint8_t *request, uint8_t *response)
{
    uint32_t port;
    
    if (*request == DAP_PORT_AUTODETECT) {
        port = DAP_DEFAULT_PORT;
    } else {
        port = *request;
    }
    switch (port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
            DAP_Data.debug_port = DAP_PORT_SWD;
            PORT_SWD_SETUP();
            break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
            //ESP_LOGI("Md-Link", "DAP_Connect: port=%d", *request);

            DAP_Data.debug_port = DAP_PORT_JTAG;
            PORT_JTAG_SETUP();
            break;
#endif
        default:
            port = DAP_PORT_DISABLED;
            break;
    }

    *response = (uint8_t)port;
    return ((1U << 16) | 1U);
}
#include "esp_io.h"
// Process Disconnect command and prepare response
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_Disconnect(uint8_t *response)
{
    DAP_Data.debug_port = DAP_PORT_DISABLED;
    PORT_OFF();

    *response = DAP_OK;
    return (1U);
}

// Process Reset Target command and prepare response
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_ResetTarget(uint8_t *response)
{
    *(response + 1) = RESET_TARGET();
    *(response + 0) = DAP_OK;
    return (2U);
}

// Process SWJ Pins command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_SWJ_Pins(const uint8_t *request, uint8_t *response)
{
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
    pMdLink_DAP_Setting_t dap_setting = get_dap_setting();
    PORT_Mode_t mode = dap_setting->swd_port_mode;
    if (mode == PORT_MODE_SPI)
        DAP_SPI_Deinit();
    uint32_t value;
    uint32_t select;
    uint32_t wait;
    uint32_t timestamp;

    value = (uint32_t)*(request + 0);
    select = (uint32_t)*(request + 1);
    wait = (uint32_t)(*(request + 2) << 0) |
           (uint32_t)(*(request + 3) << 8) |
           (uint32_t)(*(request + 4) << 16) |
           (uint32_t)(*(request + 5) << 24);

    if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
        if ((value & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
            PIN_SWCLK_TCK_SET();
        } else {
            PIN_SWCLK_TCK_CLR();
        }
    }
    if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
        if ((value & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
            PIN_SWDIO_TMS_SET();
        } else {
            PIN_SWDIO_TMS_CLR();
        }
    }
    if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
        PIN_TDI_OUT(value >> DAP_SWJ_TDI);
    }
    if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
        PIN_nTRST_OUT(value >> DAP_SWJ_nTRST);
    }
    if ((select & (1U << DAP_SWJ_nRESET)) != 0U) {
        PIN_nRESET_OUT(value >> DAP_SWJ_nRESET);
    }

    if (wait != 0U) {
#if (TIMESTAMP_CLOCK != 0U)
        if (wait > 3000000U) {
            wait = 3000000U;
        }
#if (TIMESTAMP_CLOCK >= 1000000U)
        wait *= TIMESTAMP_CLOCK / 1000000U;
#else
        wait /= 1000000U / TIMESTAMP_CLOCK;
#endif
#else
        wait = 1U;
#endif
        timestamp = TIMESTAMP_GET();
        do {
            if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
                if ((value >> DAP_SWJ_SWCLK_TCK) ^ PIN_SWCLK_TCK_IN()) {
                    continue;
                }
            }
            if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
                if ((value >> DAP_SWJ_SWDIO_TMS) ^ PIN_SWDIO_TMS_IN()) {
                    continue;
                }
            }
            if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
                if ((value >> DAP_SWJ_TDI) ^ PIN_TDI_IN()) {
                    continue;
                }
            }
            if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
                if ((value >> DAP_SWJ_nTRST) ^ PIN_nTRST_IN()) {
                    continue;
                }
            }
            if ((select & (1U << DAP_SWJ_nRESET)) != 0U) {
                if ((value >> DAP_SWJ_nRESET) ^ PIN_nRESET_IN()) {
                    continue;
                }
            }
            break;
        } while ((TIMESTAMP_GET() - timestamp) < wait);
    }

    value = (PIN_SWCLK_TCK_IN() << DAP_SWJ_SWCLK_TCK) |
            (PIN_SWDIO_TMS_IN() << DAP_SWJ_SWDIO_TMS) |
            (PIN_TDI_IN() << DAP_SWJ_TDI) |
            (PIN_TDO_IN() << DAP_SWJ_TDO) |
            (PIN_nTRST_IN() << DAP_SWJ_nTRST) |
            (PIN_nRESET_IN() << DAP_SWJ_nRESET);

    *response = (uint8_t)value;
#else
    *response = 0U;
#endif
    if (mode == PORT_MODE_SPI)
        DAP_SPI_Init();
    return ((6U << 16) | 1U);
}

extern void set_swj_clock_frequency(uint32_t clock);
// Process SWJ Clock command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_SWJ_Clock(const uint8_t *request, uint8_t *response)
{
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
    uint32_t clock;
    uint32_t delay;
    (void)delay;

    clock = (uint32_t)(*(request + 0) << 0) |
            (uint32_t)(*(request + 1) << 8) |
            (uint32_t)(*(request + 2) << 16) |
            (uint32_t)(*(request + 3) << 24);

    if (clock == 0U) {
        *response = DAP_ERROR;
        return ((4U << 16) | 1U);
    }

#if !defined(USE_SPI_SWD) && !defined(USE_SPI_JTAG)
    Set_Clock_Delay(clock);
#else
    set_swj_clock_frequency(clock);
#endif

    *response = DAP_OK;
#else
    *response = DAP_ERROR;
#endif
    pMdLink_DAP_Setting_t dap_setting = get_dap_setting();
    PORT_Mode_t mode = dap_setting->swd_port_mode;
    if (mode == PORT_MODE_SPI)
    {
        ESP_LOGI("MdLink", "DAP_SPI_Init");
        DAP_SPI_Init();
    }
    return ((4U << 16) | 1U);
}

// Process SWJ Sequence command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_SWJ_Sequence(const uint8_t *request, uint8_t *response)
{
    uint32_t count;

    count = *request++;
    if (count == 0U) {
        count = 256U;
    }

#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
    SWJ_Sequence(count, request);
    *response = DAP_OK;
#else
    *response = DAP_ERROR;
#endif

    count = (count + 7U) >> 3;

    return (((count + 1U) << 16) | 1U);
}

// Process SWD Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_SWD_Configure(const uint8_t *request, uint8_t *response)
{
#if (DAP_SWD != 0)
    uint8_t value;

    value = *request;
    DAP_Data.swd_conf.turnaround = (value & 0x03U) + 1U;
    DAP_Data.swd_conf.data_phase = (value & 0x04U) ? 1U : 0U;

    *response = DAP_OK;
#else
    *response = DAP_ERROR;
#endif

    return ((1U << 16) | 1U);
}

// Process SWD Sequence command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_SWD_Sequence(const uint8_t *request, uint8_t *response)
{
    uint32_t sequence_info;
    uint32_t sequence_count;
    uint32_t request_count;
    uint32_t response_count;
    uint32_t count;

#if (DAP_SWD != 0)
    *response++ = DAP_OK;
#else
    *response++ = DAP_ERROR;
#endif
    request_count = 1U;
    response_count = 1U;

    sequence_count = *request++;
    while (sequence_count--) {
        sequence_info = *request++;
        count = sequence_info & SWD_SEQUENCE_CLK;
        if (count == 0U) {
            count = 64U;
        }
        count = (count + 7U) / 8U;
#if (DAP_SWD != 0)
        if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
            PIN_SWDIO_OUT_DISABLE();
        } else {
            PIN_SWDIO_OUT_ENABLE();
        }
        SWD_Sequence(sequence_info, request, response);
        if (sequence_count == 0U) {
            PIN_SWDIO_OUT_ENABLE();
        }
#endif
        if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
            request_count++;
#if (DAP_SWD != 0)
            response += count;
            response_count += count;
#endif
        } else {
            request += count;
            request_count += count + 1U;
        }
    }

    return ((request_count << 16) | response_count);
}

// Process JTAG Sequence command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_JTAG_Sequence(const uint8_t *request, uint8_t *response)
{
    uint32_t sequence_info;
    uint32_t sequence_count;
    uint32_t request_count;
    uint32_t response_count;
    uint32_t count;
    //ESP_LOGI("Md-Link", "DAP_JTAG_Sequence: request=%d", *request);
#if (DAP_JTAG != 0)
    *response++ = DAP_OK;
#else
    *response++ = DAP_ERROR;
#endif
    request_count = 1U;
    response_count = 1U;

    sequence_count = *request++;
    while (sequence_count--) {
        sequence_info = *request++;
        count = sequence_info & JTAG_SEQUENCE_TCK;
        if (count == 0U) {
            count = 64U;
        }
        count = (count + 7U) / 8U;
#if (DAP_JTAG != 0)
        JTAG_Sequence(sequence_info, request, response);
#endif
        request += count;
        request_count += count + 1U;
#if (DAP_JTAG != 0)
        if ((sequence_info & JTAG_SEQUENCE_TDO) != 0U) {
            response += count;
            response_count += count;
        }
#endif
    }

    return ((request_count << 16) | response_count);
}

// Process JTAG Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_JTAG_Configure(const uint8_t *request, uint8_t *response)
{
    uint32_t count;
#if (DAP_JTAG != 0)
    uint32_t length;
    uint32_t bits;
    uint32_t n;
    //ESP_LOGI("Md-Link", "DAP_JTAG_Configure: request=%d", *request);
    count = *request++;
    DAP_Data.jtag_dev.count = (uint8_t)count;

    bits = 0U;
    for (n = 0U; n < count; n++) {
        length = *request++;
        DAP_Data.jtag_dev.ir_length[n] = (uint8_t)length;
        DAP_Data.jtag_dev.ir_before[n] = (uint16_t)bits;
        bits += length;
    }
    for (n = 0U; n < count; n++) {
        bits -= DAP_Data.jtag_dev.ir_length[n];
        DAP_Data.jtag_dev.ir_after[n] = (uint16_t)bits;
    }

    *response = DAP_OK;
#else
    count = *request;
    *response = DAP_ERROR;
#endif

    return (((count + 1U) << 16) | 1U);
}

// Process JTAG IDCODE command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_JTAG_IDCode(const uint8_t *request, uint8_t *response)
{
#if (DAP_JTAG != 0)
    uint32_t data;

    if (DAP_Data.debug_port != DAP_PORT_JTAG) {
        goto id_error;
    }

    // Device index (JTAP TAP)
    DAP_Data.jtag_dev.index = *request;
    if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
        goto id_error;
    }

    // Select JTAG chain
    JTAG_IR(JTAG_IDCODE);

    // Read IDCODE register
    data = JTAG_ReadIDCode();

    // Store Data
    *(response + 0) = DAP_OK;
    *(response + 1) = (uint8_t)(data >> 0);
    *(response + 2) = (uint8_t)(data >> 8);
    *(response + 3) = (uint8_t)(data >> 16);
    *(response + 4) = (uint8_t)(data >> 24);

    return ((1U << 16) | 5U);

id_error:
#endif
    *response = DAP_ERROR;
    return ((1U << 16) | 1U);
}

// Process Transfer Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_TransferConfigure(const uint8_t *request, uint8_t *response)
{
    DAP_Data.transfer.idle_cycles = *(request + 0);
    DAP_Data.transfer.retry_count = (uint16_t)*(request + 1) |
                                    (uint16_t)(*(request + 2) << 8);
    DAP_Data.transfer.match_retry = (uint16_t)*(request + 3) |
                                    (uint16_t)(*(request + 4) << 8);

    *response = DAP_OK;
    return ((5U << 16) | 1U);
}

// Process SWD Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_Transfer(const uint8_t *request, uint8_t *response)
{
    const uint8_t *request_head;
    uint32_t request_count;
    uint32_t request_value;
    uint8_t *response_head;
    uint32_t response_count;
    uint32_t response_value;
    uint32_t post_read;
    uint32_t check_write;
    uint32_t match_value;
    uint32_t match_retry;
    uint32_t retry;
    uint32_t data;
#if (TIMESTAMP_CLOCK != 0U)
    uint32_t timestamp;
#endif

    request_head = request;

    response_count = 0U;
    response_value = 0U;
    response_head = response;
    response += 2;

    DAP_TransferAbort = 0U;

    post_read = 0U;
    check_write = 0U;

    request++; // Ignore DAP index

    request_count = *request++;

    for (; request_count != 0U; request_count--) {
        request_value = *request++;
        if ((request_value & DAP_TRANSFER_RnW) != 0U) {
            // Read register
            if (post_read) {
                // Read was posted before
                retry = DAP_Data.transfer.retry_count;
                if ((request_value & (DAP_TRANSFER_APnDP | DAP_TRANSFER_MATCH_VALUE)) == DAP_TRANSFER_APnDP) {
                    // Read previous AP data and post next AP read
                    do {
                        response_value = SWD_Transfer(request_value, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                } else {
                    // Read previous AP data
                    do {
                        response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    post_read = 0U;
                }
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
                // Store previous AP data
                *response++ = (uint8_t)data;
                *response++ = (uint8_t)(data >> 8);
                *response++ = (uint8_t)(data >> 16);
                *response++ = (uint8_t)(data >> 24);
#if (TIMESTAMP_CLOCK != 0U)
                if (post_read) {
                    // Store Timestamp of next AP read
                    if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                        timestamp = DAP_Data.timestamp;
                        *response++ = (uint8_t)timestamp;
                        *response++ = (uint8_t)(timestamp >> 8);
                        *response++ = (uint8_t)(timestamp >> 16);
                        *response++ = (uint8_t)(timestamp >> 24);
                    }
                }
#endif
            }
            if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
                // Read with value match
                match_value = (uint32_t)(*(request + 0) << 0) |
                              (uint32_t)(*(request + 1) << 8) |
                              (uint32_t)(*(request + 2) << 16) |
                              (uint32_t)(*(request + 3) << 24);
                request += 4;
                match_retry = DAP_Data.transfer.match_retry;
                if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
                    // Post AP read
                    retry = DAP_Data.transfer.retry_count;
                    do {
                        response_value = SWD_Transfer(request_value, NULL);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    if (response_value != DAP_TRANSFER_OK) {
                        break;
                    }
                }
                do {
                    // Read register until its value matches or retry counter expires
                    retry = DAP_Data.transfer.retry_count;
                    do {
                        response_value = SWD_Transfer(request_value, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    if (response_value != DAP_TRANSFER_OK) {
                        break;
                    }
                } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
                if ((data & DAP_Data.transfer.match_mask) != match_value) {
                    response_value |= DAP_TRANSFER_MISMATCH;
                }
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
            } else {
                // Normal read
                retry = DAP_Data.transfer.retry_count;
                if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
                    // Read AP register
                    if (post_read == 0U) {
                        // Post AP read
                        do {
                            response_value = SWD_Transfer(request_value, NULL);
                        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                        if (response_value != DAP_TRANSFER_OK) {
                            break;
                        }
#if (TIMESTAMP_CLOCK != 0U)
                        // Store Timestamp
                        if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                            timestamp = DAP_Data.timestamp;
                            *response++ = (uint8_t)timestamp;
                            *response++ = (uint8_t)(timestamp >> 8);
                            *response++ = (uint8_t)(timestamp >> 16);
                            *response++ = (uint8_t)(timestamp >> 24);
                        }
#endif
                        post_read = 1U;
                    }
                } else {
                    // Read DP register
                    do {
                        response_value = SWD_Transfer(request_value, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    if (response_value != DAP_TRANSFER_OK) {
                        break;
                    }
#if (TIMESTAMP_CLOCK != 0U)
                    // Store Timestamp
                    if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                        timestamp = DAP_Data.timestamp;
                        *response++ = (uint8_t)timestamp;
                        *response++ = (uint8_t)(timestamp >> 8);
                        *response++ = (uint8_t)(timestamp >> 16);
                        *response++ = (uint8_t)(timestamp >> 24);
                    }
#endif
                    // Store data
                    *response++ = (uint8_t)data;
                    *response++ = (uint8_t)(data >> 8);
                    *response++ = (uint8_t)(data >> 16);
                    *response++ = (uint8_t)(data >> 24);
                }
            }
            check_write = 0U;
        } else {
            // Write register
            if (post_read) {
                // Read previous data
                retry = DAP_Data.transfer.retry_count;
                do {
                    response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
                } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
                // Store previous data
                *response++ = (uint8_t)data;
                *response++ = (uint8_t)(data >> 8);
                *response++ = (uint8_t)(data >> 16);
                *response++ = (uint8_t)(data >> 24);
                post_read = 0U;
            }
            // Load data
            data = (uint32_t)(*(request + 0) << 0) |
                   (uint32_t)(*(request + 1) << 8) |
                   (uint32_t)(*(request + 2) << 16) |
                   (uint32_t)(*(request + 3) << 24);
            request += 4;
            if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
                // Write match mask
                DAP_Data.transfer.match_mask = data;
                response_value = DAP_TRANSFER_OK;
            } else {
                // Write DP/AP register
                retry = DAP_Data.transfer.retry_count;
                do {
                    response_value = SWD_Transfer(request_value, &data);
                } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
#if (TIMESTAMP_CLOCK != 0U)
                // Store Timestamp
                if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                    timestamp = DAP_Data.timestamp;
                    *response++ = (uint8_t)timestamp;
                    *response++ = (uint8_t)(timestamp >> 8);
                    *response++ = (uint8_t)(timestamp >> 16);
                    *response++ = (uint8_t)(timestamp >> 24);
                }
#endif
                check_write = 1U;
            }
        }
        response_count++;
        if (DAP_TransferAbort) {
            break;
        }
    }

    for (; request_count != 0U; request_count--) {
        // Process canceled requests
        request_value = *request++;
        if ((request_value & DAP_TRANSFER_RnW) != 0U) {
            // Read register
            if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
                // Read with value match
                request += 4;
            }
        } else {
            // Write register
            request += 4;
        }
    }

    if (response_value == DAP_TRANSFER_OK) {
        if (post_read) {
            // Read previous data
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            // Store previous data
            *response++ = (uint8_t)data;
            *response++ = (uint8_t)(data >> 8);
            *response++ = (uint8_t)(data >> 16);
            *response++ = (uint8_t)(data >> 24);
        } else if (check_write) {
            // Check last write
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        }
    }

end:
    *(response_head + 0) = (uint8_t)response_count;
    *(response_head + 1) = (uint8_t)response_value;

    return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
}
#endif

// Process JTAG Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_Transfer(const uint8_t *request, uint8_t *response)
{
    const uint8_t *request_head;
    uint32_t request_count;
    uint32_t request_value;
    uint32_t request_ir;
    uint8_t *response_head;
    uint32_t response_count;
    uint32_t response_value;
    uint32_t post_read;
    uint32_t match_value;
    uint32_t match_retry;
    uint32_t retry;
    uint32_t data;
    uint32_t ir;
#if (TIMESTAMP_CLOCK != 0U)
    uint32_t timestamp;
#endif

    request_head = request;

    response_count = 0U;
    response_value = 0U;
    response_head = response;
    response += 2;

    DAP_TransferAbort = 0U;

    ir = 0U;
    post_read = 0U;

    // Device index (JTAP TAP)
    DAP_Data.jtag_dev.index = *request++;
    if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
        goto end;
    }

    request_count = *request++;

    for (; request_count != 0U; request_count--) {
        request_value = *request++;
        request_ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
        if ((request_value & DAP_TRANSFER_RnW) != 0U) {
            // Read register
            if (post_read) {
                // Read was posted before
                retry = DAP_Data.transfer.retry_count;
                if ((ir == request_ir) && ((request_value & DAP_TRANSFER_MATCH_VALUE) == 0U)) {
                    // Read previous data and post next read
                    do {
                        response_value = JTAG_Transfer(request_value, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                } else {
                    // Select JTAG chain
                    if (ir != JTAG_DPACC) {
                        ir = JTAG_DPACC;
                        JTAG_IR(ir);
                    }
                    // Read previous data
                    do {
                        response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    post_read = 0U;
                }
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
                // Store previous data
                *response++ = (uint8_t)data;
                *response++ = (uint8_t)(data >> 8);
                *response++ = (uint8_t)(data >> 16);
                *response++ = (uint8_t)(data >> 24);
#if (TIMESTAMP_CLOCK != 0U)
                if (post_read) {
                    // Store Timestamp of next AP read
                    if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                        timestamp = DAP_Data.timestamp;
                        *response++ = (uint8_t)timestamp;
                        *response++ = (uint8_t)(timestamp >> 8);
                        *response++ = (uint8_t)(timestamp >> 16);
                        *response++ = (uint8_t)(timestamp >> 24);
                    }
                }
#endif
            }
            if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
                // Read with value match
                match_value = (uint32_t)(*(request + 0) << 0) |
                              (uint32_t)(*(request + 1) << 8) |
                              (uint32_t)(*(request + 2) << 16) |
                              (uint32_t)(*(request + 3) << 24);
                request += 4;
                match_retry = DAP_Data.transfer.match_retry;
                // Select JTAG chain
                if (ir != request_ir) {
                    ir = request_ir;
                    JTAG_IR(ir);
                }
                // Post DP/AP read
                retry = DAP_Data.transfer.retry_count;
                do {
                    response_value = JTAG_Transfer(request_value, NULL);
                } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
                do {
                    // Read register until its value matches or retry counter expires
                    retry = DAP_Data.transfer.retry_count;
                    do {
                        response_value = JTAG_Transfer(request_value, &data);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    if (response_value != DAP_TRANSFER_OK) {
                        break;
                    }
                } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
                if ((data & DAP_Data.transfer.match_mask) != match_value) {
                    response_value |= DAP_TRANSFER_MISMATCH;
                }
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
            } else {
                // Normal read
                if (post_read == 0U) {
                    // Select JTAG chain
                    if (ir != request_ir) {
                        ir = request_ir;
                        JTAG_IR(ir);
                    }
                    // Post DP/AP read
                    retry = DAP_Data.transfer.retry_count;
                    do {
                        response_value = JTAG_Transfer(request_value, NULL);
                    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                    if (response_value != DAP_TRANSFER_OK) {
                        break;
                    }
#if (TIMESTAMP_CLOCK != 0U)
                    // Store Timestamp
                    if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                        timestamp = DAP_Data.timestamp;
                        *response++ = (uint8_t)timestamp;
                        *response++ = (uint8_t)(timestamp >> 8);
                        *response++ = (uint8_t)(timestamp >> 16);
                        *response++ = (uint8_t)(timestamp >> 24);
                    }
#endif
                    post_read = 1U;
                }
            }
        } else {
            // Write register
            if (post_read) {
                // Select JTAG chain
                if (ir != JTAG_DPACC) {
                    ir = JTAG_DPACC;
                    JTAG_IR(ir);
                }
                // Read previous data
                retry = DAP_Data.transfer.retry_count;
                do {
                    response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
                } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
                // Store previous data
                *response++ = (uint8_t)data;
                *response++ = (uint8_t)(data >> 8);
                *response++ = (uint8_t)(data >> 16);
                *response++ = (uint8_t)(data >> 24);
                post_read = 0U;
            }
            // Load data
            data = (uint32_t)(*(request + 0) << 0) |
                   (uint32_t)(*(request + 1) << 8) |
                   (uint32_t)(*(request + 2) << 16) |
                   (uint32_t)(*(request + 3) << 24);
            request += 4;
            if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
                // Write match mask
                DAP_Data.transfer.match_mask = data;
                response_value = DAP_TRANSFER_OK;
            } else {
                // Select JTAG chain
                if (ir != request_ir) {
                    ir = request_ir;
                    JTAG_IR(ir);
                }
                // Write DP/AP register
                retry = DAP_Data.transfer.retry_count;
                do {
                    response_value = JTAG_Transfer(request_value, &data);
                } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
                if (response_value != DAP_TRANSFER_OK) {
                    break;
                }
#if (TIMESTAMP_CLOCK != 0U)
                // Store Timestamp
                if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
                    timestamp = DAP_Data.timestamp;
                    *response++ = (uint8_t)timestamp;
                    *response++ = (uint8_t)(timestamp >> 8);
                    *response++ = (uint8_t)(timestamp >> 16);
                    *response++ = (uint8_t)(timestamp >> 24);
                }
#endif
            }
        }
        response_count++;
        if (DAP_TransferAbort) {
            break;
        }
    }

    for (; request_count != 0U; request_count--) {
        // Process canceled requests
        request_value = *request++;
        if ((request_value & DAP_TRANSFER_RnW) != 0U) {
            // Read register
            if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
                // Read with value match
                request += 4;
            }
        } else {
            // Write register
            request += 4;
        }
    }

    if (response_value == DAP_TRANSFER_OK) {
        // Select JTAG chain
        if (ir != JTAG_DPACC) {
            ir = JTAG_DPACC;
            JTAG_IR(ir);
        }
        if (post_read) {
            // Read previous data
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            // Store previous data
            *response++ = (uint8_t)data;
            *response++ = (uint8_t)(data >> 8);
            *response++ = (uint8_t)(data >> 16);
            *response++ = (uint8_t)(data >> 24);
        } else {
            // Check last write
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        }
    }

end:
    *(response_head + 0) = (uint8_t)response_count;
    *(response_head + 1) = (uint8_t)response_value;

    return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
}
#endif

// Process Dummy Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_Dummy_Transfer(const uint8_t *request, uint8_t *response)
{
    const uint8_t *request_head;
    uint32_t request_count;
    uint32_t request_value;

    request_head = request;

    request++; // Ignore DAP index

    request_count = *request++;

    for (; request_count != 0U; request_count--) {
        // Process dummy requests
        request_value = *request++;
        if ((request_value & DAP_TRANSFER_RnW) != 0U) {
            // Read register
            if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
                // Read with value match
                request += 4;
            }
        } else {
            // Write register
            request += 4;
        }
    }

    *(response + 0) = 0U; // Response count
    *(response + 1) = 0U; // Response value

    return (((uint32_t)(request - request_head) << 16) | 2U);
}

// Process Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_Transfer(const uint8_t *request, uint8_t *response)
{
    uint32_t num;

    switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
            num = DAP_SWD_Transfer(request, response);
            break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
            num = DAP_JTAG_Transfer(request, response);
            break;
#endif
        default:
            num = DAP_Dummy_Transfer(request, response);
            break;
    }

    return (num);
}

// Process SWD Transfer Block command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_TransferBlock(const uint8_t *request, uint8_t *response)
{
    uint32_t request_count;
    uint32_t request_value;
    uint32_t response_count;
    uint32_t response_value;
    uint8_t *response_head;
    uint32_t retry;
    uint32_t data;

    response_count = 0U;
    response_value = 0U;
    response_head = response;
    response += 3;

    DAP_TransferAbort = 0U;

    request++; // Ignore DAP index

    request_count = (uint32_t)(*(request + 0) << 0) |
                    (uint32_t)(*(request + 1) << 8);
    request += 2;
    if (request_count == 0U) {
        goto end;
    }

    request_value = *request++;
    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
        // Read register block
        if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
            // Post AP read
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = SWD_Transfer(request_value, NULL);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
        }
        while (request_count--) {
            // Read DP/AP register
            if ((request_count == 0U) && ((request_value & DAP_TRANSFER_APnDP) != 0U)) {
                // Last AP read
                request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
            }
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = SWD_Transfer(request_value, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            // Store data
            *response++ = (uint8_t)data;
            *response++ = (uint8_t)(data >> 8);
            *response++ = (uint8_t)(data >> 16);
            *response++ = (uint8_t)(data >> 24);
            response_count++;
        }
    } else {
        // Write register block
        while (request_count--) {
            // Load data
            data = (uint32_t)(*(request + 0) << 0) |
                   (uint32_t)(*(request + 1) << 8) |
                   (uint32_t)(*(request + 2) << 16) |
                   (uint32_t)(*(request + 3) << 24);
            request += 4;
            // Write DP/AP register
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = SWD_Transfer(request_value, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            response_count++;
        }
        // Check last write
        retry = DAP_Data.transfer.retry_count;
        do {
            response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
    }

end:
    *(response_head + 0) = (uint8_t)(response_count >> 0);
    *(response_head + 1) = (uint8_t)(response_count >> 8);
    *(response_head + 2) = (uint8_t)response_value;

    return ((uint32_t)(response - response_head));
}
#endif

// Process JTAG Transfer Block command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_TransferBlock(const uint8_t *request, uint8_t *response)
{
    uint32_t request_count;
    uint32_t request_value;
    uint32_t response_count;
    uint32_t response_value;
    uint8_t *response_head;
    uint32_t retry;
    uint32_t data;
    uint32_t ir;

    response_count = 0U;
    response_value = 0U;
    response_head = response;
    response += 3;

    DAP_TransferAbort = 0U;

    // Device index (JTAP TAP)
    DAP_Data.jtag_dev.index = *request++;
    if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
        goto end;
    }

    request_count = (uint32_t)(*(request + 0) << 0) |
                    (uint32_t)(*(request + 1) << 8);
    request += 2;
    if (request_count == 0U) {
        goto end;
    }

    request_value = *request++;

    // Select JTAG chain
    ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
    JTAG_IR(ir);

    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
        // Post read
        retry = DAP_Data.transfer.retry_count;
        do {
            response_value = JTAG_Transfer(request_value, NULL);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) {
            goto end;
        }
        // Read register block
        while (request_count--) {
            // Read DP/AP register
            if (request_count == 0U) {
                // Last read
                if (ir != JTAG_DPACC) {
                    JTAG_IR(JTAG_DPACC);
                }
                request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
            }
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = JTAG_Transfer(request_value, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            // Store data
            *response++ = (uint8_t)data;
            *response++ = (uint8_t)(data >> 8);
            *response++ = (uint8_t)(data >> 16);
            *response++ = (uint8_t)(data >> 24);
            response_count++;
        }
    } else {
        // Write register block
        while (request_count--) {
            // Load data
            data = (uint32_t)(*(request + 0) << 0) |
                   (uint32_t)(*(request + 1) << 8) |
                   (uint32_t)(*(request + 2) << 16) |
                   (uint32_t)(*(request + 3) << 24);
            request += 4;
            // Write DP/AP register
            retry = DAP_Data.transfer.retry_count;
            do {
                response_value = JTAG_Transfer(request_value, &data);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) {
                goto end;
            }
            response_count++;
        }
        // Check last write
        if (ir != JTAG_DPACC) {
            JTAG_IR(JTAG_DPACC);
        }
        retry = DAP_Data.transfer.retry_count;
        do {
            response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
    }

end:
    *(response_head + 0) = (uint8_t)(response_count >> 0);
    *(response_head + 1) = (uint8_t)(response_count >> 8);
    *(response_head + 2) = (uint8_t)response_value;

    return ((uint32_t)(response - response_head));
}
#endif

// Process Transfer Block command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_TransferBlock(const uint8_t *request, uint8_t *response)
{
    uint32_t num;

    switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
            num = DAP_SWD_TransferBlock(request, response);
            break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
            num = DAP_JTAG_TransferBlock(request, response);
            break;
#endif
        default:
            *(response + 0) = 0U; // Response count [7:0]
            *(response + 1) = 0U; // Response count[15:8]
            *(response + 2) = 0U; // Response value
            num = 3U;
            break;
    }

    if ((*(request + 3) & DAP_TRANSFER_RnW) != 0U) {
        // Read register block
        num |= 4U << 16;
    } else {
        // Write register block
        num |= (4U + (((uint32_t)(*(request + 1)) | (uint32_t)(*(request + 2) << 8)) * 4)) << 16;
    }

    return (num);
}

// Process SWD Write ABORT command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_WriteAbort(const uint8_t *request, uint8_t *response)
{
    uint32_t data;

    // Load data (Ignore DAP index)
    data = (uint32_t)(*(request + 1) << 0) |
           (uint32_t)(*(request + 2) << 8) |
           (uint32_t)(*(request + 3) << 16) |
           (uint32_t)(*(request + 4) << 24);

    // Write Abort register
    SWD_Transfer(DP_ABORT, &data);

    *response = DAP_OK;
    return (1U);
}
#endif

// Process JTAG Write ABORT command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_WriteAbort(const uint8_t *request, uint8_t *response)
{
    uint32_t data;

    // Device index (JTAP TAP)
    DAP_Data.jtag_dev.index = *request;
    if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
        *response = DAP_ERROR;
        return (1U);
    }

    // Select JTAG chain
    JTAG_IR(JTAG_ABORT);

    // Load data
    data = (uint32_t)(*(request + 1) << 0) |
           (uint32_t)(*(request + 2) << 8) |
           (uint32_t)(*(request + 3) << 16) |
           (uint32_t)(*(request + 4) << 24);

    // Write Abort register
    JTAG_WriteAbort(data);

    *response = DAP_OK;
    return (1U);
}
#endif

// Process Write ABORT command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
static uint32_t DAP_WriteAbort(const uint8_t *request, uint8_t *response)
{
    uint32_t num;

    switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
            num = DAP_SWD_WriteAbort(request, response);
            break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
            num = DAP_JTAG_WriteAbort(request, response);
            break;
#endif
        default:
            *response = DAP_ERROR;
            num = 1U;
            break;
    }
    return ((5U << 16) | num);
}

// Process DAP Vendor command request and prepare response
// Default function (can be overridden)
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response (lower 16 bits)
//             number of bytes in request (upper 16 bits)
__WEAK uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response)
{
    (void)request;
    *response = ID_DAP_Invalid;
    return ((1U << 16) | 1U);
}

/**
 * @brief 处理DAP单条命令请求并生成响应
 * @param request  输入参数，指向请求数据的指针
 * @param response 输出参数，指向响应数据的缓冲区指针
 * @return 复合返回值（32位）：
 *         - 高16位：消耗的请求字节数（包括命令ID）
 *         - 低16位：生成的响应字节数（包括状态码）
 * 
 * 此函数是DAP协议的核心分发器，根据命令ID调用对应的处理函数，
 * 支持标准CMSIS-DAP命令和厂商扩展命令。
 */

uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response)
{
    uint32_t num; // 临时变量，记录子函数返回的响应数据长度（不含命令ID）
    //ESP_LOGI("Md-Link", "DAP_ProcessCommand: request=%x", *request);
    /* ------------ 1. 厂商命令处理（0x80~0x9F）------------ */
    if ((*request >= ID_DAP_Vendor0) && (*request <= ID_DAP_Vendor31)) {
        // 0x80-0x9F范围的命令交给厂商特定处理
        return DAP_ProcessVendorCommand(request, response);
    }

    /* ------------ 2. 写入响应头（命令ID）------------ */
    *response++ = *request; // 响应首字节总是回显命令ID
    //ESP_LOGI("Md-Link", "DAP_ProcessCommand: request=%d", *request);
    /* ------------ 3. 命令分发处理 ------------ */
    //ESP_LOGI("Md-Link", "DAP_ProcessCommand: request=%d", *request);
    switch (*request++) { // 注意：这里会移动request指针
        /* ----- 基础调试命令 ----- */
        case ID_DAP_Info: // 0x00：获取信息
            // 特殊处理：第二字节是信息类型，响应数据需要额外长度字段
            num = DAP_Info(*request, response + 1); // 执行命令处理
            *response = (uint8_t)num;               // 在响应第二字节写入数据长度
            // 返回字节数：请求2字节（ID+type），响应2+num字节（ID+len+data）
            return ((2U << 16) | (2U + num));

        case ID_DAP_HostStatus: // 0x01：主机状态控制
            num = DAP_HostStatus(request, response);
            break;

        /* ----- 连接管理 ----- */
        case ID_DAP_Connect: // 0x02：连接目标设备
            num = DAP_Connect(request, response);
            break;
        case ID_DAP_Disconnect: // 0x03：断开连接
            num = DAP_Disconnect(response);
            break;

        /* ----- 控制命令 ----- */
        case ID_DAP_Delay: // 0x09：延时操作
            num = DAP_Delay(request, response);
            break;
        case ID_DAP_ResetTarget: // 0x0A：目标复位
            num = DAP_ResetTarget(response);
            break;

        /* ----- SWD/JTAG接口命令 ----- */
        case ID_DAP_SWJ_Pins: // 0x10：引脚控制
            num = DAP_SWJ_Pins(request, response);
            break;
        case ID_DAP_SWJ_Clock: // 0x11：时钟设置
            num = DAP_SWJ_Clock(request, response);
            break;
        case ID_DAP_SWJ_Sequence: // 0x12：序列生成
            num = DAP_SWJ_Sequence(request, response);
            break;

        /* ----- SWD特定命令 ----- */
        case ID_DAP_SWD_Configure: // 0x13：SWD配置
            num = DAP_SWD_Configure(request, response);
            break;
        case ID_DAP_SWD_Sequence: // 0x14：SWD序列
            num = DAP_SWD_Sequence(request, response);
            break;

        /* ----- JTAG特定命令 ----- */
        case ID_DAP_JTAG_Sequence: // 0x15：JTAG序列
            num = DAP_JTAG_Sequence(request, response);
            break;
        case ID_DAP_JTAG_Configure: // 0x16：JTAG配置
            num = DAP_JTAG_Configure(request, response);
            break;
        case ID_DAP_JTAG_IDCODE: // 0x17：读取IDCODE
            num = DAP_JTAG_IDCode(request, response);
            break;

        /* ----- 数据传输命令 ----- */
        case ID_DAP_TransferConfigure: // 0x04：传输配置
            num = DAP_TransferConfigure(request, response);
            break;
        case ID_DAP_Transfer: // 0x05：单次传输
            num = DAP_Transfer(request, response);
            break;
        case ID_DAP_TransferBlock: // 0x06：块传输
            num = DAP_TransferBlock(request, response);
            break;
        case ID_DAP_WriteABORT: // 0x08：写ABORT寄存器
            num = DAP_WriteAbort(request, response);
            break;
            /* ----- SWO跟踪命令（条件编译）----- */
#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
        case ID_DAP_SWO_Transport: // 0x17：传输模式
            num = SWO_Transport(request, response);
            break;
        case ID_DAP_SWO_Mode: // 0x21：工作模式
            num = SWO_Mode(request, response);
            break;
        case ID_DAP_SWO_Baudrate:// 0x22：波特率设置
            num = SWO_Baudrate(request, response);
            break;
        case ID_DAP_SWO_Control:
            num = SWO_Control(request, response);
            break;
        case ID_DAP_SWO_Status:
            num = SWO_Status(response);
            break;
        case ID_DAP_SWO_ExtendedStatus:
            num = SWO_ExtendedStatus(request, response);
            break;
        case ID_DAP_SWO_Data:
            num = SWO_Data(request, response);
            break;
#endif
            /* ----- UART桥接命令（条件编译）----- */
#if (DAP_UART != 0)
        case ID_DAP_UART_Transport: // 0x30：UART传输模式
            num = UART_Transport(request, response);
            break;
        case ID_DAP_UART_Configure:
            num = UART_Configure(request, response);
            break;
        case ID_DAP_UART_Control:
            num = UART_Control(request, response);
            break;
        case ID_DAP_UART_Status:
            num = UART_Status(response);
            break;
        case ID_DAP_UART_Transfer:
            num = UART_Transfer(request, response);
            break;
#endif

        /* ----- 无效命令处理 ----- */
        default:
            *(response - 1) = ID_DAP_Invalid; // 修改响应头为无效命令标识
            return ((1U << 16) | 1U);         // 请求1字节，响应1字节
    }

    /* ------------ 4. 标准命令返回格式 ------------ */
    // 默认返回格式：请求1字节（ID），响应1+num字节（ID+data）
    return ((1U << 16) + (1U + num));
}
/**
 * @brief 执行DAP调试命令的核心函数
 * @param request  输入参数，指向请求数据的指针
 * @param response 输出参数，指向响应数据的缓冲区指针
 * @return 复合返回值（32位）：
 *         - 高16位：处理的请求字节数
 *         - 低16位：生成的响应字节数
 * 
 * 此函数处理两种类型的DAP命令：
 * 1. 单条普通命令（直接调用DAP_ProcessCommand处理）
 * 2. 批量执行的命令队列（ID_DAP_ExecuteCommands）
 */
uint32_t DAP_ExecuteCommand(const uint8_t *request, uint8_t *response)
{
    uint32_t cnt, num, n; // 临时变量：
                          // cnt - 批量命令计数器
                          // num - 字节数统计器
                          // n - 单次处理的字节数

    // 检查是否为批量命令执行请求（0x04）
    if (*request == ID_DAP_ExecuteCommands) {
        // 步骤1：准备响应头
        *response++ = *request++;   // 回传命令ID（0x04）
        cnt = *request++;           // 获取子命令数量
        *response++ = (uint8_t)cnt; // 回传子命令数量
    //ESP_LOGI("Md-Link", "DAP_ExecuteCommand: cnt=%d", cnt);
        // 初始化字节计数器：
        // 高16位：已处理2字节请求头（ID + cnt）
        // 低16位：已生成2字节响应头
        num = (2U << 16) | 2U;

        // 步骤2：循环处理每个子命令
        while (cnt--) {
            // 处理单个子命令（递归调用）
            n = DAP_ProcessCommand(request, response);

            // 更新总字节数：
            num += n; // 高16位自动累加请求字节数
                      // 低16位自动累加响应字节数

            // 移动指针：
            request += (uint16_t)(n >> 16); // 按请求字节数前进
            response += (uint16_t)n;        // 按响应字节数前进
        }
        return (num); // 返回总字节数（高16位：请求，低16位：响应）
    }

    // 普通单条命令直接处理
    return DAP_ProcessCommand(request, response);
}

// Setup DAP
void DAP_Setup(void)
{
    // Default settings
    DAP_Data.debug_port = 0U;
    DAP_Data.transfer.idle_cycles = 0U;
    DAP_Data.transfer.retry_count = 100U;
    DAP_Data.transfer.match_retry = 0U;
    DAP_Data.transfer.match_mask = 0x00000000U;
#if (DAP_SWD != 0)
    DAP_Data.swd_conf.turnaround = 1U;
    DAP_Data.swd_conf.data_phase = 0U;
#endif
#if (DAP_JTAG != 0)
    DAP_Data.jtag_dev.count = 0U;
#endif

    // Sets DAP_Data.fast_clock and DAP_Data.clock_delay.
    Set_Clock_Delay(DAP_DEFAULT_SWJ_CLOCK);

    DAP_SETUP(); // Device specific setup
}
